// _Exercise4_BackgroundSubstraction_Avg2.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

int threshold = 17;
float learningRate = 0.05;
int g_slider_position = 5;

float kParameter = 1;


float** mean;
float** covariance;


void onTrackbarSlide(int pos)
{
	learningRate = g_slider_position / 100.0;
}

void RunningGaussianAvg(IplImage* Icurrent,IplImage* Iresult,float learning, int maxValue)
{
	cvZero(Iresult);
	int widthStep = Icurrent->widthStep;
	int channel = Icurrent->nChannels;

	uchar* IcurrentData = (uchar*)Icurrent->imageData;
	uchar* IresultData = (uchar*)Iresult->imageData;

	for(int i = 0; i < Icurrent->height; i++ )
	{
		for(int j = 0 ; j < Icurrent->width; j ++)
		{
			for(int k = 0; k < Icurrent->nChannels; k++)
			{
				int index = i*widthStep + j * channel + k;
				
				int temp = IcurrentData[index] - mean[i][j];
				if(temp < 0)
					temp = -temp;
				if(temp > kParameter*sqrt(covariance[i][j]) + 1)
				{
					IresultData[index] = maxValue;
					covariance[i][j] = learning*temp*temp + (1-learning)*covariance[i][j];
				}
				else
				{
					IresultData[index] = 0;	//background										
				}
				mean[i][j] = learning*IcurrentData[index] + (1-learning)*mean[i][j];

			}
		}
	}
}


int _tmain(int argc, _TCHAR* argv[])
{
	cvNamedWindow("Sample",CV_WINDOW_AUTOSIZE);
	cvNamedWindow("GreyScale",CV_WINDOW_AUTOSIZE);
	cvNamedWindow("Subtraction",CV_WINDOW_AUTOSIZE);
	cvNamedWindow("LearningSubstraction",CV_WINDOW_AUTOSIZE);

	CvCapture* capture= cvCreateFileCapture("D:\\Leavy4a_dataset.avi");
	IplImage* Iprev = cvQueryFrame(capture);

	mean = new float* [Iprev->height];
	covariance = new float* [Iprev->height];
	


	if(!Iprev)
	{
		printf("Error in open file");
		return 0;
	}


	cvCreateTrackbar("Threshold","Subtraction",&threshold,40,NULL);
	cvCreateTrackbar("Learning Rate","LearningSubstraction",&g_slider_position,100,onTrackbarSlide);


	IplImage* IprevGrey = cvCreateImage(cvSize(Iprev->width,Iprev->height),Iprev->depth,1);
	cvCvtColor(Iprev,IprevGrey,CV_RGB2GRAY);

	for(int i = 0 ; i < IprevGrey->height; i++)
	{
		mean[i] = new float [IprevGrey->width];
		covariance[i] = new float [IprevGrey->width];
		for(int j = 0; j < IprevGrey->width; j++)
		{
//			int index = i*IprevGrey->widthStep + j*IprevGrey->nChannels;
			mean[i][j] = 0;//IprevGrey->imageData[index];
			covariance[i][j] = 0;
		}
	}

	cvShowImage("Sample",Iprev);
	cvShowImage("GreyScale",IprevGrey);

	int fps = cvGetCaptureProperty(capture,CV_CAP_PROP_FPS);
	printf("Frame-per-second is %d\n",fps);


	IplImage* Icurrent;
	IplImage* Iprev2 = cvCreateImage(cvSize(Iprev->width,Iprev->height),Iprev->depth,3);
	cvCopyImage(Iprev,Iprev2);
	IplImage* IcurrentGrey = cvCreateImage(cvSize(Iprev->width,Iprev->height),Iprev->depth,1);


	while(1)
	{
		//////////////////////////////////////////////////////////////////////////
		// BASIC SUBTRACTION
		//////////////////////////////////////////////////////////////////////////
		Icurrent = cvQueryFrame(capture);
		cvCvtColor(Icurrent,IcurrentGrey,CV_RGB2GRAY);

		IplImage* IFrameForeground = cvCreateImage(cvSize(Iprev->width,Iprev->height),Iprev->depth,1);

		cvAbsDiff(IcurrentGrey,IprevGrey,IFrameForeground);
		cvThreshold(IFrameForeground,IFrameForeground,threshold,255,CV_THRESH_BINARY);

		if(!Icurrent)
			break;

		cvShowImage("Sample",Icurrent);
		cvShowImage("GreyScale",IcurrentGrey);
		cvShowImage("Subtraction",IFrameForeground);
		cvCopyImage(IcurrentGrey,IprevGrey);


		//////////////////////////////////////////////////////////////////////////
		// BASIC SUBTRACTION WITH LEARNING
		//////////////////////////////////////////////////////////////////////////
		IplImage* IFrameForeGround2 = cvCreateImage(cvSize(Iprev->width,Iprev->height),Iprev->depth,1);
		IplImage* Icurrent2 = cvCreateImage(cvSize(Iprev->width,Iprev->height),Iprev->depth,3);
		cvCopyImage(Icurrent,Icurrent2);

		IplImage* IcurrentGrey2 = cvCreateImage(cvSize(Iprev->width,Iprev->height),Iprev->depth,1);
		IplImage* IprevGrey2 = cvCreateImage(cvSize(Iprev->width,Iprev->height),Iprev->depth,1);

		cvCvtColor(Icurrent2,IcurrentGrey2,CV_RGB2GRAY);
//		cvCvtColor(Iprev2,IprevGrey2,CV_RGB2GRAY);
//		cvAbsDiff(IcurrentGrey2,IprevGrey2,IFrameForeGround2); 		
//		cvThreshold(IFrameForeGround2,IFrameForeGround2,threshold,255,CV_THRESH_BINARY);
		RunningGaussianAvg(IcurrentGrey2,IFrameForeGround2,learningRate,255);
		cvShowImage("LearningSubstraction",IFrameForeGround2);
//		UpdateBackground(Iprev2,Icurrent2,Iprev2,learningRate);
//		RunningGaussianAvg(Iprev2,Icurrent2,Iprev2,learningRate);


		char c = cvWaitKey(fps);
		if(c == 27)
			break;
	}

	cvReleaseImage(&IprevGrey);
	cvReleaseCapture(&capture);
	cvDestroyWindow("Sample");
	cvDestroyWindow("GreyScale");
	cvDestroyWindow("Subtraction");
	
	return 0;
}

